using System.Collections.Generic;
using UnityEngine;

public class Graph
{
    public List<Edge> Edges = new List<Edge>();
    public List<Node> nodes = new List<Node>();

    public Graph()
    {
    }

    public void AddNode(OctreeNode otn)
    {
        if (FindNode(otn.Id) == null)
        {
            Node node = new Node(otn);
            nodes.Add(node);
        }
    }

    public void AddEdge(OctreeNode formNode, OctreeNode toNode)
    {
        Node from = FindNode(formNode.Id);
        Node to = FindNode(toNode.Id);
        if (from != null && to != null)
        {
            // 创建双向图
            Edge edge = new Edge(from, to);
            Edges.Add(edge);
            from.edgeList.Add(edge);
            Edge f = new Edge(to, from);
            Edges.Add(f);
            to.edgeList.Add(f);
        }
    }

    // public void Draw()
    // {
    //     for (int i = 0; i < Edges.Count; i++)
    //     {
    //         Debug.DrawLine(Edges[i].startNode.octreeNode.nodeBounds.center,
    //             Edges[i].endNode.octreeNode.nodeBounds.center,
    //             Color.red
    //         );
    //     }
    //
    //     for (int i = 0; i < nodes.Count; i++)
    //     {
    //         Gizmos.color = Color.yellow;
    //         Gizmos.DrawWireSphere(nodes[i].octreeNode.nodeBounds.center, 0.25f);
    //     }
    // }

    public Node FindNode(int id)
    {
        foreach (Node node in nodes)
        {
            if (node.GetNode().Id == id)
            {
                return node;
            }
        }

        return null;
    }

    public bool AStar(OctreeNode startNode, OctreeNode endNode, List<Node> pathList)
    {
        pathList.Clear();
        Node start = FindNode(startNode.Id);
        Node end = FindNode(endNode.Id);

        if (start == null || end == null)
        {
            return false;
        }

        // 初始化所有节点
        foreach (Node node in nodes)
        {
            node.g = float.MaxValue;
            node.h = 0;
            node.f = float.MaxValue;
            node.cameFrom = null;
        }

        List<Node> open = new List<Node>();
        List<Node> close = new List<Node>();

        start.g = 0;
        start.h = Vector3.Distance(startNode.nodeBounds.center, endNode.nodeBounds.center);
        start.f = start.g + start.h;

        open.Add(start);

        while (open.Count > 0)
        {
            int i = lowestF(open);
            Node current = open[i];
            
            // 找到终点
            if (current.octreeNode.Id == endNode.Id)
            {
                reconstructPath(current, pathList);
                return true;
            }

            open.RemoveAt(i);
            close.Add(current);

            foreach (Edge e in current.edgeList)
            {
                Node neighbour = e.endNode;
                
                // 跳过已关闭的节点
                if (close.Contains(neighbour))
                    continue;

                // 计算从起点经过当前节点到邻居的实际代价
                float tentative_g = current.g + Vector3.Distance(
                    current.octreeNode.nodeBounds.center, 
                    neighbour.octreeNode.nodeBounds.center);

                // 如果邻居不在开放列表中，添加到开放列表
                if (!open.Contains(neighbour))
                {
                    open.Add(neighbour);
                }
                // 如果这不是更好的路径，跳过
                else if (tentative_g >= neighbour.g)
                {
                    continue;
                }

                // 这是一个更好的路径，记录它
                neighbour.cameFrom = current;
                neighbour.g = tentative_g;
                neighbour.h = Vector3.Distance(
                    neighbour.octreeNode.nodeBounds.center, 
                    endNode.nodeBounds.center);
                neighbour.f = neighbour.g + neighbour.h;
            }
        }

        return false;
    }

    // 重构路径 - 从终点回溯到起点
    public void reconstructPath(Node endNode, List<Node> pathList)
    {
        pathList.Clear();
        Node current = endNode;
        
        while (current != null)
        {
            pathList.Insert(0, current);
            current = current.cameFrom;
        }
    }

    int lowestF(List<Node> l)
    {
        if (l.Count == 0) return -1;

        int minIndex = 0;
        float minF = l[0].f;

        for (int i = 1; i < l.Count; i++)
        {
            if (l[i].f < minF)
            {
                minF = l[i].f;
                minIndex = i;
            }
        }

        return minIndex;
    }
}